#include <esp_now.h>
#include <WiFi.h>

// 电机引脚定义（四轮差速驱动）
#define MOTOR_A_IN1 25
#define MOTOR_A_IN2 16
#define MOTOR_B_IN1 18
#define MOTOR_B_IN2 17
#define MOTOR_C_IN1 21
#define MOTOR_C_IN2 19
#define MOTOR_D_IN1 23
#define MOTOR_D_IN2 22

// 超声波模块引脚定义
const int TRIG_PIN = 26;
const int ECHO_PIN = 27;

// 参数配置
const float SOUND_SPEED = 0.0343; // cm/μs (340m/s)
const unsigned long TIMEOUT = 100000; // 超时时间100ms
float targetDistance = 7.0;    // 目标跟随距离（cm）
const float followDeadZone = 1.0; // 跟随死区范围±1cm
const float Kp = 30.0;         // 比例系数（根据实际调整）

// 模式类型
typedef enum
{
  eModeType_Normal =0,  // 遥控模式
  eModeType_Follow =1,  // 超声波跟随模式
}EModeType;

// 摇杆数据结构体（发送端与接收端需一致）
typedef struct {
  int xValueJpyA;    // X轴值（0-4095）
  int yValueJpyA;    // Y轴值（0-4095）
  int xValueJpyB;    // X轴值（0-4095）
  int yValueJpyB;    // Y轴值（0-4095）
  EModeType modeType;
} JoystickData;
JoystickData joyData;

// 差速转向参数
const int compen = 12;    // 补偿值 （不动时推杆为 0，-12，0,0）
const int deadZone = 10;  //死区
const int baseSpeed = 150;   // 基础速度（0-255）
const int followSpeed= 50;   //超声波跟随模式速度

// ==================== 新增循迹相关配置 ====================
#define TRACK_L2 4     // 最左侧传感器
#define TRACK_L1 5     // 左侧传感器
#define TRACK_M 33     // 中间传感器
#define TRACK_R1 34    // 右侧传感器
#define TRACK_R2 35    // 最右侧传感器

const float Kp_track = 20.0;     // 循迹比例系数（需实际调试）
const int trackBaseSpeed = 150;  // 循迹基础速度
const int trackDeadZone = 1;     // 偏差死区阈值


// 循迹模块
void track()
{
  // 1. 读取五路传感器状态（0=检测到黑线）
  int sL2 = !digitalRead(TRACK_L2); // 使用上拉输入，检测到黑线时为0
  int sL1 = !digitalRead(TRACK_L1);
  int sM  = !digitalRead(TRACK_M);
  int sR1 = !digitalRead(TRACK_R1);
  int sR2 = !digitalRead(TRACK_R2);

  Serial.println("sR2:");
  Serial.println(sR2);
}


// 跟随模式控制逻辑
void handleFollowMode(float currentDistance) {
  // 计算距离误差
  float error = currentDistance - targetDistance;
  
  // 死区处理
  if (fabs(error) <= followDeadZone) {
    // 停止所有电机
    setMotor(MOTOR_A_IN1, MOTOR_A_IN2, 0);
    setMotor(MOTOR_B_IN1, MOTOR_B_IN2, 0);
    setMotor(MOTOR_C_IN1, MOTOR_C_IN2, 0);
    setMotor(MOTOR_D_IN1, MOTOR_D_IN2, 0);
    return;
  }

  // 比例控制计算速度
  int speed = constrain(Kp * error, -followSpeed, followSpeed);

  // 设置所有电机同速（前后直行）
  setMotor(MOTOR_A_IN1, MOTOR_A_IN2, speed);
  setMotor(MOTOR_B_IN1, MOTOR_B_IN2, speed);
  setMotor(MOTOR_C_IN1, MOTOR_C_IN2, speed);
  setMotor(MOTOR_D_IN1, MOTOR_D_IN2, speed);
}

// 控制单个电机（速度范围：-255~+255）
void setMotor(int in1, int in2, int speed) {
    speed = constrain(speed, -255, 255);
    if (speed > 0) {
      analogWrite(in1, speed);
      analogWrite(in2, 0);
    } else {
      analogWrite(in1, 0);
      analogWrite(in2, -speed);
    }
  }

  float getDistanceCM() 
  {
  // 发送10μs触发脉冲
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  // 测量回波高电平持续时间
  unsigned long duration = pulseIn(ECHO_PIN, HIGH, TIMEOUT);
  
  // 计算距离（cm）
  return duration * SOUND_SPEED / 2;
 }

  // 差速控制函数（完整实现）
void differentialDrive(float throttle, float steering, int* leftSpeed, int* rightSpeed) {
  /* 参数说明：
     throttle : 油门量（-255~+255）
     steering : 转向量（-100~+100）
     leftSpeed: 输出左轮速度
     rightSpeed: 输出右轮速度
  */
  
  // 转向系数计算（限制在±1之间）
  float steeringFactor = constrain(steering, -100, 100) / 100.0f;

  // 差速计算（核心算法）
  *leftSpeed = throttle * (1.0f + steeringFactor);
  *rightSpeed = throttle * (1.0f - steeringFactor);

  // 动态限幅（保持比例关系）
  int maxVal = max(abs(*leftSpeed), abs(*rightSpeed));
  if (maxVal > 255) {
      float scale = 255.0f / maxVal;
      *leftSpeed *= scale;
      *rightSpeed *= scale;
  }
}

  /* 遥感
        100 
  100 ------ -100   x   
        -100
         y
  */
  
  // 接收回调函数
  void OnDataRecv(const uint8_t *mac, const uint8_t *data, int len) {
    memcpy(&joyData, data, len);

    // 在setup()中添加：
    //Serial.print("结构体大小应为18，实际：");
    //Serial.println(sizeof(joyData));
    
    // 获取遥感参数
    float xA = joyData.xValueJpyA+compen;  // A遥感补偿值
    float yA = joyData.yValueJpyA;  
    float xB = joyData.xValueJpyB;  
    float yB = joyData.yValueJpyB;
    EModeType modeType = joyData.modeType;
    //Serial.print("mode is："); 
    //Serial.println(modeType);


    if(modeType == eModeType_Normal)
    {
      // 死区处理
      if (abs(xA) < deadZone) xA = 0;
      if (abs(yA) < deadZone) yA = 0;
      if (abs(xB) < deadZone) xB = 0;
      if (abs(yB) < deadZone) yB = 0;
      
      // 按照A遥感Y值作为速度值
      // 速度映射（保持yA的-100~100比例）
      float throttle = yA * (baseSpeed / 100.0);
      
      // 差速控制计算（新增）
      int leftFront, rightFront, leftRear, rightRear;
      differentialDrive(throttle, xB, &rightFront, &leftFront);
    
      // 四轮同步控制（前后轮同速）
      leftRear = leftFront;
      rightRear = rightFront;
  
      // 遥感B负责方向
      
      // 设置电机速度
      setMotor(MOTOR_A_IN1, MOTOR_A_IN2, leftFront);
      setMotor(MOTOR_B_IN1, MOTOR_B_IN2, rightFront);
      setMotor(MOTOR_C_IN1, MOTOR_C_IN2, leftRear);
      setMotor(MOTOR_D_IN1, MOTOR_D_IN2, rightRear);
    }
    else
    {
       // 超声波跟谁模式
       // 获取当前距离（需在主循环更新）
       static float currentDistance = 0;
       currentDistance = getDistanceCM();

      if(joyData.modeType == eModeType_Follow)
      {
        // 距离有效性检查
        if(currentDistance > 50 || currentDistance < 2) 
        {
          // 停止并报警
          handleFollowMode(targetDistance); // 强制停止
          Serial.println("无效距离，停止跟随");
          return;
        }
        handleFollowMode(currentDistance);
      }
   }
 }

void setup() {
  Serial.begin(115200);
  // 初始化电机引脚为PWM输出
  pinMode(MOTOR_A_IN1, OUTPUT);
  pinMode(MOTOR_A_IN2, OUTPUT);
  
  pinMode(MOTOR_B_IN1, OUTPUT);
  pinMode(MOTOR_B_IN2, OUTPUT);

  pinMode(MOTOR_C_IN1, OUTPUT);
  pinMode(MOTOR_C_IN2, OUTPUT);

  pinMode(MOTOR_D_IN1, OUTPUT);
  pinMode(MOTOR_D_IN2, OUTPUT);

   // 初始化循迹传感器引脚（上拉输入模式）
  pinMode(TRACK_L2, INPUT);
  pinMode(TRACK_L1, INPUT);
  pinMode(TRACK_M, INPUT);
  pinMode(TRACK_R1, INPUT);
  pinMode(TRACK_R2, INPUT);

  // 超声波模块引脚初始化
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  digitalWrite(TRIG_PIN, LOW); // 初始保持低电平
  delay(100); // 模块启动稳定时间
  
  // ESP-NOW初始化
  WiFi.mode(WIFI_STA);
  if (esp_now_init() != ESP_OK) 
  {
    Serial.println("ESP-NOW初始化失败");
    return;
  }

  // 注册接收回调
  esp_now_register_recv_cb(OnDataRecv);

  setMotor(MOTOR_A_IN1, MOTOR_A_IN2, 0); 
  setMotor(MOTOR_B_IN1, MOTOR_B_IN2, 0);
  setMotor(MOTOR_C_IN1, MOTOR_C_IN2, 0);
  setMotor(MOTOR_D_IN1, MOTOR_D_IN2, 0);
}


void loop()
{
  track();
}
//#include <WiFi.h>
//
//void setup() {
//Serial.begin(115200);
//WiFi.mode(WIFI_MODE_STA);
//Serial.println(WiFi.macAddress());
//}
//
//void loop() {
//}
